/**
 * 2024/05/21
 * ssyyw
 * mavproxy_ros1的飞行任务处理节点
 *
 */

#ifndef DRONE_LISTENER
#define DRONE_LISTENER

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include "mavproxy_ros1/MavproxyState.h"
#include "drone_defs.h"

class DroneListener{
public:
    DroneListener();
    void Run();

    void StateCallback(const mavros_msgs::State::ConstPtr &msg);
    void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void VelocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);
    void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &msg);
    void FlushMavproxyState();

private:
    mavros_msgs::State drone_current_state;
    geographic_msgs::GeoPoint drone_current_gpose;
    geometry_msgs::PoseStamped drone_current_pose;
    geometry_msgs::TwistStamped drone_current_velocity;

    ros::Subscriber drone_state_sub;
    ros::Subscriber drone_current_pos_sub;
    ros::Subscriber drone_current_gpspos_sub;
    ros::Subscriber drone_current_velocity_sub;

    ros::Publisher mavproxyStatePub;

    mavproxy_ros1::MavproxyState mavproxy_state;
    int sent_seq = 0;

    ros::NodeHandle nh;
};

#endif